#include "containment.h"

namespace Packing 
{

	//double Containment::lambda = 1.0;

	void Containment::clear()
	{
		const_list_.clear();
	}

	void Containment::insert_constraint(const Point_2& p,const Segment_2& seg, double dist)
	{
		const_list_.push_back(Constraint(p, seg,  dist));
	}

	void Containment::centralize_constraints(const Point_2& local_origin)
	{
		Transformation_2 cent(CGAL::TRANSLATION, Vector_2(local_origin, CGAL::ORIGIN));
		for (unsigned int i = 0; i < const_list_.size(); i++)
		{
			const_list_[i].p_ = const_list_[i].p_.transform(cent);
			const_list_[i].seg_ = const_list_[i].seg_.transform(cent);
		}
	}
	void Containment::compute_constraints(const double * const x, double* c, int mc)
	{
		assert(mc == const_list_.size());
		// x[]: k theta t0 t1
		double fcos = cos(x[1]); 
		double fsin = sin(x[1]); 
		Transformation_2 aff(x[0]*fcos, -x[0]*fsin, x[2], x[0]*fsin,  x[0]*fcos, x[3]);
		for (unsigned int i=0; i<const_list_.size(); i++)
			c[i] = const_list_[i].transformed_signed_area(aff); // >=0	
	}

	void Containment::compute_constraint_grads(const double * const x, double* jac)
	{
		double fcos = cos(x[1]); 
		double fsin = sin(x[1]); 
		Transformation_2 rotate(fcos, -fsin, 0, fsin,  fcos, 0);
		Transformation_2 aff(-x[0]*fsin, -x[0]*fcos, 0,	x[0]*fcos,  -x[0]*fsin, 0);

		int idx =0;
		for (int i=0; i<const_list_.size(); i++)
		{
			Point_2 pp = rotate(const_list_[i].p_);
			jac[idx] = (pp-CGAL::ORIGIN)*const_list_[i].n_-const_list_[i].dist_;

			pp = aff(const_list_[i].p_);
			jac[idx+1] = (pp-CGAL::ORIGIN)*const_list_[i].n_;

			jac[idx+2] = const_list_[i].n_.x();
			jac[idx+3] = const_list_[i].n_.y();

			idx += 4;
		}
	}

} 

